#include "ogl/math/Util.h"
#include "Util.h"

using namespace Ogl::Math;

Plane Util::CreatePlaneFrom(const Transform &t)
{
    Plane res;
    res.normal = t.GetUp();
    float D = -glm::dot(t.position, res.normal);
    res.distance = -D;

    return res;
};

bool Util::RayCast(const Plane &plane, const Ray &ray, glm::vec3 &pos)
{
    float dist = plane.getSignedDistanceToPlane(ray.origin);
    float cosTheta = glm::dot(ray.direction, plane.normal);
    pos = -dist * ray.direction / (cosTheta) + ray.origin;

    return cosTheta < 0;
}

glm::mat4 Util::Mirror(const Plane &plane, const glm::mat4 &model)
{
    glm::mat4 planeTranslationMatrix = glm::translate(glm::mat4(1.0f), -plane.normal * plane.distance);
    glm::mat4 mirrorMatrix = glm::mat4(1.0f);
    mirrorMatrix[0][0] = 1.0f - 2.0f * plane.normal.x * plane.normal.x;
    mirrorMatrix[0][1] = -2.0f * plane.normal.x * plane.normal.y;
    mirrorMatrix[0][2] = -2.0f * plane.normal.x * plane.normal.z;

    mirrorMatrix[1][0] = -2.0f * plane.normal.y * plane.normal.x;
    mirrorMatrix[1][1] = 1.0f - 2.0f * plane.normal.y * plane.normal.y;
    mirrorMatrix[1][2] = -2.0f * plane.normal.y * plane.normal.z;

    mirrorMatrix[2][0] = -2.0f * plane.normal.z * plane.normal.x;
    mirrorMatrix[2][1] = -2.0f * plane.normal.z * plane.normal.y;
    mirrorMatrix[2][2] = 1.0f - 2.0f * plane.normal.z * plane.normal.z;

    glm::mat4 inversePlaneTranslationMatrix = glm::translate(glm::mat4(1.0f), plane.normal * plane.distance);

    glm::mat4 mirrorModelMatrix = inversePlaneTranslationMatrix * mirrorMatrix * planeTranslationMatrix * model;

    return mirrorModelMatrix;
}
